

#include <math.h>
#include "../nr/nrutil.h"

// =============================================================================

/*
 * Singular Value Decomposition code from Numerical Recipes in C Second Edition.
 */
double pythag(double a, double b) {
	/*Computes (a2 + b2 )1/2 without destructive underflow or overflow. */
	double absa, absb;
	absa = fabs(a);
	absb = fabs(b);
	if (absa > absb) return absa * sqrt(1.0 + SQR(absb / absa));
	else return (absb == 0.0 ? 0.0 : absb * sqrt(1.0 + SQR(absa / absb)));
}
void svdcmp(double *a, int m, int n, double *w, double *v) {

	/* Given a matrix a[1..m][1..n], this routine computes its singular value decomposition, A =
	U ·W ·V T . The matrix U replaces a on output. The diagonal matrix of singular values W is out -
	put as a vector w[1..n]. The matrix V(not the transpose V T) is output as v[1..n][1..n].*/

	//double pythag(double a, double b);
	int flag, i, its, j, jj, k, l, nm;
	double anorm, c, f, g, h, s, scale, x, y, z, *rv1;
	rv1 = vector(1, n);
	g = scale = anorm = 0.0;
	// Householder reduction to bidiagonal form.
	for (i = 1; i <= n; i++) {
		l = i + 1;
		rv1[i] = scale*g;
		g = s = scale = 0.0;
		if (i <= m) {
			for (k = i; k <= m; k++) scale += fabs(a[k * n + i]);
			if (scale) {
				for (k = i; k <= m; k++) {
					a[k * n + i] /= scale;
					s += a[k * n + i] * a[k * n + i];
				}
				f = a[i * n + i];
				g = -SIGN(sqrt(s), f);
				h = f * g - s;
				a[i * n + i] = f - g;
				for (j = l; j <= n; j++) {
					for (s = 0.0, k = i; k <= m; k++) s += a[k * n + i] * a[k * n + j];
					f = s / h;
					for (k = i; k <= m; k++) a[k * n + j] += f * a[k * n + i];
				}
				for (k = i; k <= m; k++) a[k * n + i] *= scale;
			}
		}
		w[i] = scale *g;
		g = s = scale = 0.0;
		if (i <= m && i != n) {
			for (k = l; k <= n; k++) scale += fabs(a[i * n + k]);
			if (scale) {
				for (k = l; k <= n; k++) {
					a[i * n + k] /= scale;
					s += a[i * n + k] * a[i * n + k];
				}
				f = a[i * n + l];
				g = -SIGN(sqrt(s), f);
				h = f * g - s;
				a[i * n + l] = f - g;
				for (k = l; k <= n; k++) rv1[k] = a[i * n + k] / h;
				for (j = l; j <= m; j++) {
					for (s = 0.0, k = l; k <= n; k++) s += a[j * n + k] * a[i * n + k];
					for (k = l; k <= n; k++) a[j * n + k] += s * rv1[k];
				}
				for (k = l; k <= n; k++) a[i * n + k] *= scale;
			}
		}
		anorm = FMAX(anorm, (fabs(w[i]) + fabs(rv1[i])));
	}
	for (i = n; i >= 1; i--) { // Accumulation of right - hand transformations.

		if (i < n) {
			if (g) {
				for (j = l; j <= n; j++) // double division to avoid possible underflow.
					v[j * n + i] = (a[i * n + j] / a[i * n + l]) / g;
				for (j = l; j <= n; j++) {
					for (s = 0.0, k = l; k <= n; k++) s += a[i * n + k] * v[k * n + j];
					for (k = l; k <= n; k++) v[k * n + j] += s * v[k * n + i];
				}
			}
			for (j = l; j <= n; j++) v[i * n + j] = v[j * n + i] = 0.0;
		}
		v[i * n + i] = 1.0;
		g = rv1[i];
		l = i;
	}
	for (i = IMIN(m, n); i >= 1; i--) { // Accumulation of left - hand transformations.

		l = i + 1;
		g = w[i];
		for (j = l; j <= n; j++) a[i * n + j] = 0.0;
		if (g) {
			g = 1.0 / g;
			for (j = l; j <= n; j++) {
				for (s = 0.0, k = l; k <= m; k++) s += a[k * n + i] * a[k * n + j];
				f = (s / a[i * n + i]) * g;
				for (k = i; k <= m; k++) a[k * n + j] += f * a[k * n + i];
			}
			for (j = i; j <= m; j++) a[j * n + i] *= g;
		} else for (j = i; j <= m; j++) a[j * n + i] = 0.0;
		++a[i * n + i];
	}
	for (k = n; k >= 1; k--) { // Diagonalization of the bidiagonal form : Loop over
		for (its = 1; its <= 30; its++) { // singular values, and over allowed iterations.
			flag = 1;
			for (l = k; l >= 1; l--) { // Test for splitting.
				nm = l - 1; //Note that rv1[1] is always zero.
				if ((double) (fabs(rv1[l]) + anorm) == anorm) {
					flag = 0;
					break;
				}
				if ((double) (fabs(w[nm]) + anorm) == anorm) break;
			}
			if (flag) {
				c = 0.0; // Cancellation of rv1[l], if l > 1.
				s = 1.0;
				for (i = l; i <= k; i++) {
					f = s * rv1[i];
					rv1[i] = c * rv1[i];
					if ((double) (fabs(f) + anorm) == anorm) break;
					g = w[i];
					h = pythag(f, g);
					w[i] = h;
					h = 1.0 / h;
					c = g*h;
					s = -f*h;
					for (j = 1; j <= m; j++) {
						y = a[j * n + nm];
						z = a[j * n + i];
						a[j * n + nm] = y * c + z*s;
						a[j * n + i] = z * c - y*s;
					}
				}
			}
			z = w[k];
			if (l == k) { // Convergence.
				if (z < 0.0) { // Singular value is made nonnegative.
					w[k] = -z;
					for (j = 1; j <= n; j++) v[j * n + k] = -v[j * n + k];
				}
				break;
			}
			if (its == 30) nrerror("no convergence in 30 svdcmp iterations");
			x = w[l]; // Shift from bottom 2 - by - 2 minor.
			nm = k - 1;
			y = w[nm];
			g = rv1[nm];
			h = rv1[k];
			f = ((y - z)*(y + z)+(g - h)*(g + h)) / (2.0 * h * y);
			g = pythag(f, 1.0);
			f = ((x - z)*(x + z) + h * ((y / (f + SIGN(g, f))) - h)) / x;
			c = s = 1.0; // Next QR transformation :
			for (j = l; j <= nm; j++) {
				i = j + 1;
				g = rv1[i];
				y = w[i];
				h = s*g;
				g = c*g;
				z = pythag(f, h);
				rv1[j] = z;
				c = f / z;
				s = h / z;
				f = x * c + g*s;
				g = g * c - x*s;
				h = y*s;
				y *= c;
				for (jj = 1; jj <= n; jj++) {
					x = v[jj * n + j];
					z = v[jj * n +  i];
					v[jj * n + j] = x * c + z*s;
					v[jj * n +  i] = z * c - x*s;
				}
				z = pythag(f, h);
				w[j] = z; // Rotation can be arbitrary if z = 0.
				if (z) {
					z = 1.0 / z;
					c = f*z;
					s = h*z;
				}
				f = c * g + s*y;
				x = c * y - s*g;
				for (jj = 1; jj <= m; jj++) {
					y = a[jj * n + j];
					z = a[jj * n + i];
					a[jj * n + j] = y * c + z*s;
					a[jj * n + i] = z * c - y*s;
				}
			}
			rv1[l] = 0.0;
			rv1[k] = f;
			w[k] = x;
		}
	}
	free_vector(rv1, 1, n);
}

// =============================================================================

/*  svd.c -- Singular value decomposition. Translated to 'C' from the
 *           original Algol code in "Handbook for Automatic Computation,
 *           vol. II, Linear Algebra", Springer-Verlag.
 *
 *  (C) 2000, C. Bond. All rights reserved.
 *
 *  This is almost an exact translation from the original, except that
 *  an iteration counter is added to prevent stalls. This corresponds
 *  to similar changes in other translations.
 *
 *  Returns an error code = 0, if no errors and 'k' if a failure to
 *  converge at the 'kth' singular value.
 * 
 */
int __svd_hac_bond(int m, int n, int withu, int withv, double eps, double tol, double **a, double *q, double **u, double **v) {
	int i, j, k, l, l1, iter, retval;
	double c, f, g, h, s, x, y, z;
	double *e;

	e = (double *) calloc(n, sizeof (double));
	retval = 0;

	/* Copy 'a' to 'u' */
	for (i = 0; i < m; i++) {
		for (j = 0; j < n; j++)
			u[i][j] = a[i][j];
	}
	/* Householder's reduction to bidiagonal form. */
	g = x = 0.0;
	for (i = 0; i < n; i++) {
		e[i] = g;
		s = 0.0;
		l = i + 1;
		for (j = i; j < m; j++)
			s += (u[j][i] * u[j][i]);
		if (s < tol)
			g = 0.0;
		else {
			f = u[i][i];
			g = (f < 0) ? sqrt(s) : -sqrt(s);
			h = f * g - s;
			u[i][i] = f - g;
			for (j = l; j < n; j++) {
				s = 0.0;
				for (k = i; k < m; k++)
					s += (u[k][i] * u[k][j]);
				f = s / h;
				for (k = i; k < m; k++)
					u[k][j] += (f * u[k][i]);
			} /* end j */
		} /* end s */
		q[i] = g;
		s = 0.0;
		for (j = l; j < n; j++)
			s += (u[i][j] * u[i][j]);
		if (s < tol)
			g = 0.0;
		else {
			f = u[i][i + 1];
			g = (f < 0) ? sqrt(s) : -sqrt(s);
			h = f * g - s;
			u[i][i + 1] = f - g;
			for (j = l; j < n; j++)
				e[j] = u[i][j] / h;
			for (j = l; j < m; j++) {
				s = 0.0;
				for (k = l; k < n; k++)
					s += (u[j][k] * u[i][k]);
				for (k = l; k < n; k++)
					u[j][k] += (s * e[k]);
			} /* end j */
		} /* end s */
		y = fabs(q[i]) + fabs(e[i]);
		if (y > x)
			x = y;
	} /* end i */

	/* accumulation of right-hand transformations */
	if (withv) {
		for (i = n - 1; i >= 0; i--) {
			if (g != 0.0) {
				h = u[i][i + 1] * g;
				for (j = l; j < n; j++)
					v[j][i] = u[i][j] / h;
				for (j = l; j < n; j++) {
					s = 0.0;
					for (k = l; k < n; k++)
						s += (u[i][k] * v[k][j]);
					for (k = l; k < n; k++)
						v[k][j] += (s * v[k][i]);

				} /* end j */
			} /* end g */
			for (j = l; j < n; j++)
				v[i][j] = v[j][i] = 0.0;
			v[i][i] = 1.0;
			g = e[i];
			l = i;
		} /* end i */

	} /* end withv, parens added for clarity */

	/* accumulation of left-hand transformations */
	if (withu) {
		for (i = n; i < m; i++) {
			for (j = n; j < m; j++)
				u[i][j] = 0.0;
			u[i][i] = 1.0;
		}
	}
	if (withu) {
		for (i = n - 1; i >= 0; i--) {
			l = i + 1;
			g = q[i];
			for (j = l; j < m; j++) /* upper limit was 'n' */
				u[i][j] = 0.0;
			if (g != 0.0) {
				h = u[i][i] * g;
				for (j = l; j < m; j++) { /* upper limit was 'n' */
					s = 0.0;
					for (k = l; k < m; k++)
						s += (u[k][i] * u[k][j]);
					f = s / h;
					for (k = i; k < m; k++)
						u[k][j] += (f * u[k][i]);
				} /* end j */
				for (j = i; j < m; j++)
					u[j][i] /= g;
			}/* end g */
			else {
				for (j = i; j < m; j++)
					u[j][i] = 0.0;
			}
			u[i][i] += 1.0;
		} /* end i*/
	} /* end withu, parens added for clarity */

	/* diagonalization of the bidiagonal form */
	eps *= x;
	for (k = n - 1; k >= 0; k--) {
		iter = 0;
test_f_splitting:
		for (l = k; l >= 0; l--) {
			if (fabs(e[l]) <= eps) goto test_f_convergence;
			if (fabs(q[l - 1]) <= eps) goto cancellation;
		} /* end l */

		/* cancellation of e[l] if l > 0 */
cancellation:
		c = 0.0;
		s = 1.0;
		l1 = l - 1;
		for (i = l; i <= k; i++) {
			f = s * e[i];
			e[i] *= c;
			if (fabs(f) <= eps) goto test_f_convergence;
			g = q[i];
			h = q[i] = sqrt(f * f + g * g);
			c = g / h;
			s = -f / h;
			if (withu) {
				for (j = 0; j < m; j++) {
					y = u[j][l1];
					z = u[j][i];
					u[j][l1] = y * c + z * s;
					u[j][i] = -y * s + z * c;
				} /* end j */
			} /* end withu, parens added for clarity */
		} /* end i */
test_f_convergence:
		z = q[k];
		if (l == k) goto convergence;

		/* shift from bottom 2x2 minor */
		iter++;
		if (iter > 30) {
			retval = k;
			break;
		}
		x = q[l];
		y = q[k - 1];
		g = e[k - 1];
		h = e[k];
		f = ((y - z)*(y + z) + (g - h)*(g + h)) / (2 * h * y);
		g = sqrt(f * f + 1.0);
		f = ((x - z)*(x + z) + h * (y / ((f < 0) ? (f - g) : (f + g)) - h)) / x;
		/* next QR transformation */
		c = s = 1.0;
		for (i = l + 1; i <= k; i++) {
			g = e[i];
			y = q[i];
			h = s * g;
			g *= c;
			e[i - 1] = z = sqrt(f * f + h * h);
			c = f / z;
			s = h / z;
			f = x * c + g * s;
			g = -x * s + g * c;
			h = y * s;
			y *= c;
			if (withv) {
				for (j = 0; j < n; j++) {
					x = v[j][i - 1];
					z = v[j][i];
					v[j][i - 1] = x * c + z * s;
					v[j][i] = -x * s + z * c;
				} /* end j */
			} /* end withv, parens added for clarity */
			q[i - 1] = z = sqrt(f * f + h * h);
			c = f / z;
			s = h / z;
			f = c * g + s * y;
			x = -s * g + c * y;
			if (withu) {
				for (j = 0; j < m; j++) {
					y = u[j][i - 1];
					z = u[j][i];
					u[j][i - 1] = y * c + z * s;
					u[j][i] = -y * s + z * c;
				} /* end j */
			} /* end withu, parens added for clarity */
		} /* end i */
		e[l] = 0.0;
		e[k] = f;
		q[k] = x;
		goto test_f_splitting;
convergence:
		if (z < 0.0) {
			/* q[k] is made non-negative */
			q[k] = -z;
			if (withv) {
				for (j = 0; j < n; j++)
					v[j][k] = -v[j][k];
			} /* end withv, parens added for clarity */
		} /* end z */
	} /* end k */

	free(e);
	return retval;
}

// =============================================================================

